#include "Serial/Serial.hpp"

communication::communication() :
    rclcpp::Node("Serial")
{
    INIT();
    pose_publisher_ = create_publisher<geometry_msgs::msg::Pose2D>(pos_topic_.c_str(), 10);
    pose_subscription_ = create_subscription<geometry_msgs::msg::Pose2D>(leaser_pos.c_str(), 10, std::bind(&communication::Pose_Re_callback, this, std::placeholders::_1));
    Timer_01 = create_wall_timer(std::chrono::milliseconds(1000), std::bind(&communication::Timer_Callback_01, this));
}
communication::~communication()
{
    if (serial_.isOpen())
    {
        serial_.close();
    }
}
/**
 * 发送格式化数据包
 * @param serial 已打开的串口对象引用
 * @param int_value 要发送的16位整数
 * @param float_value 要发送的32位浮点数
 * @return 成功返回发送的字节数，失败返回-1
 */
int communication::sendPacket(serial::Serial &serial, int16_t int_value, float float_value)
{
    // 构建数据包
    std::vector<uint8_t> packet;

    // 添加包头
    packet.push_back(0xA5);

    // 添加int数据 (2字节，小端序)
    packet.push_back(static_cast<uint8_t>(int_value & 0xFF));
    packet.push_back(static_cast<uint8_t>((int_value >> 8) & 0xFF));

    // 添加float数据 (4字节，小端序)
    uint8_t *float_bytes = reinterpret_cast<uint8_t *>(&float_value);
    for (int i = 0; i < 4; i++)
    {
        packet.push_back(float_bytes[i]);
    }

    // 计算校验和 (数据和的低8位，不包含包头)
    uint8_t checksum = 0;
    for (size_t i = 1; i < packet.size(); i++)
    {
        checksum += packet[i];
    }
    packet.push_back(checksum);

    // 添加包尾
    packet.push_back(0x5A);

    // 打印完整数据包
    std::stringstream ss;
    ss << "发送数据包: ";
    for (uint8_t byte : packet)
    {
        ss << std::hex << std::setw(2) << std::setfill('0') << static_cast<int>(byte) << " ";
    }
    ss << "| INT: " << std::dec << int_value
       << " | FLOAT: " << float_value;
    // std::cout << ss.str() << std::endl;
    RCLCPP_INFO(this->get_logger(), "%s", ss.str().c_str());

    // 发送数据包
    try
    {
        size_t written = serial.write(packet);
        return static_cast<int>(written);
    } catch (const serial::IOException &e)
    {
        // 处理发送错误
        return -1;
    }
}
void communication::Timer_Callback_01()
{
    static int num = 0;

    RCLCPP_INFO(this->get_logger(), "Xu_Hua : 第 %d 次 : communication 已经生成 !!!", num++);
    // 数据初始值
    static int16_t int_data = 0;
    static float float_data = 0.0f;

    // 发送数据包
    int result = sendPacket(serial_, int_data, float_data);
    if (result > 0)
    {
        // 发送成功，递增数据
        int_data++;
        float_data += 1.0f;
    } else
    {
        // 处理发送失败
    }
}
bool communication::INIT()
{
    const int max_retries = 5; // 最大重试次数
    int retry_count = 0;       // 重试计数器

    while (rclcpp::ok())
    {
        try
        {
            // 配置串口参数（建议移至初始化函数外部，避免重复设置）
            serial_.setPort(serial_port_);
            serial_.setBaudrate(serial_baud_);
            serial_.setFlowcontrol(serial::flowcontrol_none);
            serial_.setParity(serial::parity_none);
            serial_.setStopbits(serial::stopbits_one);
            serial_.setBytesize(serial::eightbits);
            serial::Timeout time_out = serial::Timeout::simpleTimeout(serial_timeout_);
            serial_.setTimeout(time_out);
            serial_.open();
            // 尝试打开串口（新增超时等待）
            if (serial_.isOpen())
            {
                RCLCPP_INFO(this->get_logger(), "Serial Port initialized successfully");
                sign = 0; // 标记串口已打开
                break;    // 成功后立即退出循环
            }
        } catch (const std::exception &e)
        {
            RCLCPP_ERROR(this->get_logger(), "Serial port open failed: %s. Retrying...", e.what());
        }

        // 串口打开失败时的处理
        if (!serial_.isOpen())
        {
            RCLCPP_ERROR(this->get_logger(), "Failed to initialize serial port (Attempt %d/\u221E)",
                         retry_count + 1);
            retry_count++; // 增加重试计数

            // 等待一段时间再重试（避免CPU占用过高）
            rclcpp::sleep_for(std::chrono::milliseconds(1000));
        }
    }

    // 处理最终结果
    if (!serial_.isOpen())
    {
        RCLCPP_FATAL(this->get_logger(), "Failed to open serial port after %d retries. Exiting.", max_retries);
        return false; // 初始化失败
    }

    // 发送初始数据（建议在串口打开后立即发送一次，避免空发送）
    uint8_t txdata[12] = {0};
    send_data(txdata, sizeof(txdata));

    RCLCPP_INFO(this->get_logger(), "Serial port initialized and begin sending data");

    return true; // 初始化成功
}

void communication::receive_port(uint8_t h, uint8_t c, uint8_t l, uint8_t t)
{
    size_t size;
    while (rclcpp::ok())
    {
        uint8_t head[1] = {0xff};
        size = serial_.read(head, 1); // 接收头

        if (head[0] != h)
        {
            RCLCPP_ERROR(this->get_logger(), "head ERROR %X", head[0]);
            continue;
        }

        uint8_t comd[1] = {0xff};
        size = serial_.read(comd, 1); // 命令位

        if (comd[0] != c)
        {
            RCLCPP_ERROR(this->get_logger(), "comd ERROR");
            continue;
        }

        uint8_t length[1] = {0xff};
        size = serial_.read(length, 1); // 数据长度

        if (length[0] != l)
        {
            RCLCPP_ERROR(this->get_logger(), "length ERROR");
            continue;
        }

        std::vector<uint8_t> fd_data(length[0] + 1, 0xff);  // 使用 std::vector 替代 VLA
        size = serial_.read(fd_data.data(), length[0] + 1); // 数据区域和尾

        // 解析数据
        if (fd_data[length[0]] == t) // 判断末尾标识
        {
            POS.x = (short)((short)fd_data[0] << 8 | fd_data[1]);
            POS.y = (short)((short)fd_data[2] << 8 | fd_data[3]);
            POS.theta = ((short)((short)fd_data[4] << 8 | fd_data[5])) / 10.f;
            pose_publisher_->publish(POS);
            RCLCPP_INFO(this->get_logger(), "READ DATA");
            break;
        }
        break;
    }
}

void communication::send_data(const uint8_t *data, size_t length)
{
    serial_.write(data, length); // 使用指针和长度
}

void communication::Pose_Re_callback(geometry_msgs::msg::Pose2D::SharedPtr get_pos)
{
    receive_port(0xAB, 0xAC, 0x06, 0xAE);

    int x = get_pos->x;
    int y = get_pos->y;
    uint8_t txdata[12];

    txdata[0] = 0xAB;
    txdata[1] = 0xAC;
    txdata[2] = 0x08;

    txdata[3] = x & 0xFF;
    txdata[4] = (x >> 8) & 0xFF;
    txdata[5] = (x >> 16) & 0xFF;
    txdata[6] = (x >> 24) & 0xFF;

    txdata[7] = y & 0xFF;
    txdata[8] = (y >> 8) & 0xFF;
    txdata[9] = (y >> 16) & 0xFF;
    txdata[10] = (y >> 24) & 0xFF;

    txdata[11] = 0xAE;
    send_data(txdata, sizeof(txdata)); // 传递数组和长度

    for (int i = 0; i < 12; i++)
    {
        printf("Send:%X\r\n", txdata[i]);
    }
}

int main(int argc, char **argv)
{
    rclcpp::init(argc, argv);
    auto node = std::make_shared<communication>();

    rclcpp::spin(node);
    rclcpp::shutdown();
    return 0;
}